//
// Created by cy on 22-5-4.
//
#include "trance.hpp"

#include <utility>
#include "except.hpp"

using namespace std;
using namespace spdlog;
using namespace LibSerial;

struct sutSTM32_DATA_PTR {
    uchar headBuff;
    uchar level;
    float pitch;
    float yaw;
    uchar shootV;
    uchar team;
    uchar changeFlag;
};

struct sutNUC_DATA_PTR {
    uchar headBuff;
    short pitch;
    short yaw;
    uchar null1;
    uchar null2;
};

class TrancePorterPrivate {

public:
    explicit TrancePorterPrivate(string port, BaudRate rate, int timeout);

    SerialStream m_serDevStream;

    int m_timeout;
    string m_port;
    BaudRate m_rate;

    shared_ptr<logger> m_outLogger;
    shared_ptr<logger> m_errLogger;

};

TrancePorterPrivate::TrancePorterPrivate(string port, BaudRate rate, int timeout)
        : m_serDevStream(), m_timeout(timeout), m_port(std::move(port)), m_rate(rate) {
    m_outLogger = stdout_color_st("TranceOut");
    m_outLogger->set_level(level::info);
    m_outLogger->flush_on(level::info);

    m_errLogger = stderr_color_st("TranceErr");
    m_errLogger->set_level(level::err);
    m_errLogger->flush_on(level::err);
}

TrancePorter::TrancePorter(const string &port, BaudRate rate, int timeout) : m_ptr(
        new TrancePorterPrivate(port, rate, timeout)) {
}

TrancePorter::~TrancePorter() {
    delete m_ptr;
}

sutStm32Data TrancePorter::read() {
    static bool changeFlag = false;
    auto &dev = m_ptr->m_serDevStream;
    if (!dev.IsDataAvailable()) {
        return {};
    } else {
        char buffer[16];
        uchar pitch[4], yaw[4];
        dev.read(buffer, 16);
        if (uchar(buffer[0]) != 0xff)
            return {};
        for (int i = 0; i < 4; i++) {
            pitch[i] = buffer[i + 2];
            yaw[i] = buffer[i + 6];
        }
        sutSTM32_DATA_PTR ptrData{
            uchar(buffer[0]), uchar(buffer[1]),
            *(float *) pitch, *(float *) yaw,
            uchar(buffer[10]), uchar(buffer[11]), uchar(buffer[12])
        };

        sutStm32Data data = {};
        data.colorType = ptrData.team == 0x01 ? enuArmorColor::COLOR_RED : enuArmorColor::COLOR_BLUE;
        if (ptrData.changeFlag) {
            changeFlag = ~changeFlag;
        }
        data.predictFlag = changeFlag;
        data.pitch = ptrData.pitch;
        data.yaw = ptrData.yaw;
        if (ptrData.shootV) {
            data.shootV = ptrData.shootV;
        } else if (TrancePorter::SPEED_TYPE == TYPE_BUST) {
            data.shootV = ptrData.level == 0x01 ? 15 : 18;
        } else {
            data.shootV = 29;
        }
        data.isOk = true;
        return data;
    }
}

void TrancePorter::write(const sutNucData &data) {
    auto &dev = m_ptr->m_serDevStream;
    sutNUC_DATA_PTR ptrData = {0xff, short(data.pitch * 100), short(data.yaw * 100), 0, 0};
    char buffer[100];
    memcpy(buffer, &ptrData, sizeof(sutNUC_DATA_PTR));
    dev.write(buffer, sizeof(sutNUC_DATA_PTR));
}

void TrancePorter::open() {
    auto &dev = m_ptr->m_serDevStream;
    dev.SetBaudRate(m_ptr->m_rate);
    dev.Open(m_ptr->m_port);
    if (dev.IsOpen()) {
        m_ptr->m_outLogger->info("串口打开成功!");
    } else {
        m_ptr->m_errLogger->error("串口打开失败!");
        throw CecException("串口打开失败!");
    }
}

void TrancePorter::close() {
    auto &dev = m_ptr->m_serDevStream;
    dev.Close();
    if (!dev.IsOpen()) {
        m_ptr->m_outLogger->info("串口关闭成功!");
    } else {
        m_ptr->m_errLogger->error("串口关闭失败!");
    }
}

TrancePorter *TrancePorter::getInstance(const string &port, LibSerial::BaudRate rate, int timeout) {
    static TrancePorter instance(port, rate, timeout);
    return &instance;
}

